#include "command_parse.h"
#include <QDataStream>
#include "command_define.h"
#include <QDebug>
#include <bitset>
#include <QtEndian>


CommandParse::CommandParse()
{

}

QByteArray CommandParse::BuildSendData(const quint16 &frame, const unsigned char &camera_id, const unsigned char &cmd, const QByteArray &data)
{
    QByteArray send_message;
    QByteArray dataheader((const char*)&Command::sPreambleSignature, sizeof(Command::sPreambleSignature));
    send_message.append(dataheader);
    QByteArray frame_array = ShortToArray(frame);
    send_message.append(frame_array);
    send_message.append(camera_id);
    send_message.append(cmd);

#ifdef BigAndSmall
    QByteArray length = IntToArray(qFromBigEndian(data.size()));
#else
    QByteArray length = IntToArray(data.size());
#endif
    send_message.append(length);
    send_message.append(data);
    unsigned char ExclusiveOrCheck = xorCheck(send_message.mid(2,send_message.size()));  //异或校验
    unsigned char SumCheck = sumCheck(send_message.mid(2,send_message.size()));         //加和校验
    send_message.append(ExclusiveOrCheck);
    send_message.append(SumCheck);
    QByteArray dataend((const char*)&Command::sEndSignature, sizeof(Command::sEndSignature));
    send_message.append(dataend);
    return send_message;
}

bool CommandParse::ParseCommands(QByteArray &src_data, QList<QByteArray> &list_data)
{
    //    QByteArray temp_src = src_data;
        while (src_data.length() >= ReceiveDataMinBytes()) {
            if(memcmp((const char*)src_data.data(), Command::sPreambleSignature, 2) != 0 ){
                src_data.remove(0, 1);
                continue;
            }

            QByteArray cmd;
            if(!ParseOneCommand(src_data, cmd)){
                src_data.remove(0, src_data.length());
            }else{
                list_data.append(cmd);
                src_data.remove(0, cmd.length());
    //            src_data = src_data;
            }
        }

        return true;
}
//一个分组除具体参数外的其他构成大小
int CommandParse::ReceiveDataMinBytes()
{
    return sizeof(Command::CommandHeader) + 2*sizeof(unsigned char) + sizeof(Command::CommandEnd);
}

bool CommandParse::ParseOneCommand(QByteArray &src_data, QByteArray &parsed_data)
{
    if(memcmp((const char*)src_data.data(), Command::sPreambleSignature, 2) != 0 ){
        return false;
    }

    int min_bytes = ReceiveDataMinBytes();
    if (src_data.length() < min_bytes)
    {
        return false;
    }
//    qDebug()<<"src.data"<<src_data.toHex();
    const char* p_data = (const char*)src_data.data();

    QString str_error;
    QByteArray header;
    header.resize(sizeof(Command::CommandHeader));

    memcpy(header.data(), p_data, sizeof(Command::CommandHeader));
#ifdef BigAndSmall
    unsigned int data_size = qFromBigEndian(ArrayToInt(header.mid(6, sizeof (unsigned int)))) + ReceiveDataMinBytes();
#else
    unsigned int data_size = ArrayToInt(header.mid(6, sizeof (unsigned int))) + ReceiveDataMinBytes();
#endif
//    qDebug() << qFromBigEndian(ArrayToInt(header.mid(6, sizeof (unsigned int)))) <<header.mid(6, sizeof (unsigned int))<< "[[[[[[[[[[[[[[[[[[[[";
    parsed_data.resize(data_size);
    memcpy(parsed_data.data(), p_data, data_size);

    unsigned char xor_data = *parsed_data.mid(data_size - 4, 1).data();
    unsigned char sum_data = *parsed_data.mid(data_size - 3, 1).data();
    unsigned char check_xor_data = xorCheck(parsed_data.mid(2, parsed_data.size() - 6));
    unsigned char check_sum_data = sumCheck(parsed_data.mid(2, parsed_data.size() - 6));
    if (xor_data != check_xor_data ||
            sum_data != check_sum_data)
    {
        qDebug()<<"crc wrong" << check_xor_data << check_sum_data;
        return false;
    }
    return true;
}

unsigned char CommandParse::GetCommandId(const QByteArray &command)
{
    if(command.size() >= ReceiveDataMinBytes()){
        int index = sizeof (Command::CommandHeader::preamble) + sizeof (Command::CommandHeader::frameNum) + sizeof (Command::CommandHeader::cameraId);
//        qDebug()<<"cccc"<<*reinterpret_cast<uchar*>(command.mid(index, sizeof(Command::CommandHeader::codeNum)).data());
        return *reinterpret_cast<uchar*>(command.mid(index, sizeof(Command::CommandHeader::codeNum)).data());
    }
    return -1;
}

bool CommandParse::CheckMatchCommand(const QByteArray &cmd1, const QByteArray &cmd2)
{
    if(cmd1.size() == cmd2.size()){
        const char* p_data1 = (const char*)cmd1.data();
        const char* p_data2 = (const char*)cmd2.data();

        int frameNum = sizeof (Command::CommandHeader::preamble);
        int camera_id = sizeof (Command::CommandHeader::preamble) + sizeof (Command::CommandHeader::frameNum);
        int code_num = sizeof (Command::CommandHeader::preamble) + sizeof (Command::CommandHeader::frameNum) + sizeof (Command::CommandHeader::cameraId);
        if(p_data1[frameNum] == p_data2[frameNum] &&
                p_data1[camera_id] == p_data2[camera_id] &&
                    p_data1[code_num] == GetRecvCode(p_data2[code_num])){
            return true;
        }
    }

    return false;
}

unsigned char CommandParse::sumCheck(QByteArray array)
{
    unsigned char iSum = 0;
    for (int i = 0;i < array.size();i++)
    {
        iSum += array[i];
    }
    //        iSum %= 0x100;   //也可以&0xff
    return iSum;
}

unsigned char CommandParse::xorCheck(QByteArray array)
{
    unsigned char checksum = 0;
    for (int i = 0;i < array.size();i++)
    {
        checksum = checksum ^ array[i];
    }
    return checksum;
}

QByteArray CommandParse::IntToArray(unsigned int v)
{
    int size = sizeof (unsigned int);

    QByteArray a;
    a.resize(size);

    for(int i = 0; i < size; i++){
        a[i] = static_cast<char>( (v >> ( (size - i -1) * 8) ) & 0xff );
    }

    return a;
}

QByteArray CommandParse::ShortToArray(unsigned short v)
{
    int size = sizeof (unsigned short);

    QByteArray a;
    a.resize(size);

    for(int i = 0; i < size; i++){
        a[i] = static_cast<char>( (v >> ( (size - i -1) * 8) ) & 0xff );
    }

    return a;
}

unsigned int CommandParse::ArrayToInt(const QByteArray &in)
{
    if (in.size()==0)
    {
        return -1;
    }
    else
    {
        bool ok = false;
        int ret = in.toHex().toUInt(&ok, 16);
        if(ok)
        {
            return ret;
        }
        return -1;
    }
}

unsigned char CommandParse::GetRecvCode(unsigned char a)
{
    std::bitset<8> b = a;

    for(int i = 4; i < 8; i++){
        b[i] = ~b[i];
    }
    unsigned char res = static_cast<unsigned char>(b.to_ulong());
    return res;
}
